#include <ros/package.h>
#include "ros_utility.hpp"


int main(int argc, char **argv)
{
    std::string pointcloudTopic;
    std::string lidarOdomTopic;
    std::vector<std::string> imageTopics;
    std::vector<std::string> filteredClasses;
    int filterRadius; // 过滤半径
    std::string outputDir;
    std::cout.precision(20); // 设置输出精度
    std::string packageName = "adapter";
    ros::init(argc, argv, packageName);
    std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>("~");

    bool useInterpolation; // 是否使用插值
    std::string packagePath = ros::package::getPath(packageName);
    nh->param<bool>("use_interpolation", useInterpolation, false);
    nh->param<std::string>("pointcloud_topic", pointcloudTopic, "/livox/lidar");
    nh->param<std::string>("lidar_odom_topic", lidarOdomTopic, "/odom");
    nh->param<std::vector<std::string>>("image_topics", imageTopics, {});
    nh->param<std::vector<std::string>>("filtered_classes", filteredClasses, {});
    nh->param<int>("filter_radius", filterRadius, 0);
    nh->param<std::string>("output_dir", outputDir, packagePath + "/result/");
    ROS_INFO("pointcloud_topic: %s", pointcloudTopic.c_str());
    ROS_INFO("lidar_odom_topic: %s", lidarOdomTopic.c_str());
    ROS_INFO("image_topics: %s", imageTopics.size() > 1 ? (imageTopics[0] + " " + imageTopics[1]).c_str() : "None");
    ROS_INFO("output_dir: %s", outputDir.c_str());
    ROS_INFO("Filtered Classes: %s", 
        filteredClasses.empty() ? "None" : 
        std::accumulate(filteredClasses.begin(), filteredClasses.end(), std::string(),
                        [](const std::string& a, const std::string& b) { return a + (a.empty() ? "" : ", ") + b; }).c_str());
    ROS_INFO("Filter Radius: %d", filterRadius);

    LidarCameraParam lidarCamParam;

    for (int i = 0; i < imageTopics.size(); i++) {
        std::vector<double> distortionCoeffs;
        std::vector<double> intrinsics;
        std::vector<int> resolution;
        std::vector<int> outResolution;
        std::vector<double> vectorBuf;

        std::string camId = std::to_string(i);
        nh->param<std::vector<double>>("cam" + camId + "/distortion_coeffs", distortionCoeffs, std::vector<double>());
        nh->param<std::vector<double>>("cam" + camId + "/intrinsics", intrinsics, std::vector<double>());
        nh->param<std::vector<int>>("cam" + camId + "/resolution", resolution, std::vector<int>());
        nh->param<std::vector<int>>("cam" + camId + "/out_resolution", outResolution, std::vector<int>());
        nh->param<std::vector<double>>("cam"+camId+"/T_lidar_cam", vectorBuf, std::vector<double>());

        ROS_INFO("cam%d/distortionCoeffs: %s", i, distortionCoeffs.size() > 1 ? (std::to_string(distortionCoeffs[0]) + " " + std::to_string(distortionCoeffs[1]) + " " + std::to_string(distortionCoeffs[2]) + " " + std::to_string(distortionCoeffs[3])).c_str() : "None");
        ROS_INFO("cam%d/intrinsics: %s", i, intrinsics.size() > 1 ? (std::to_string(intrinsics[0]) + " " + std::to_string(intrinsics[1]) + " " + std::to_string(intrinsics[2]) + " " + std::to_string(intrinsics[3])).c_str() : "None");
        ROS_INFO("cam%d/resolution: %s", i, resolution.size() > 1 ? (std::to_string(resolution[0]) + " " + std::to_string(resolution[1])).c_str() : "None");
        ROS_INFO("cam%d/out_resolution: %s", i, outResolution.size() > 1 ? (std::to_string(outResolution[0]) + " " + std::to_string(outResolution[1])).c_str() : "None");
        ROS_INFO("cam%d/T_lidar_cam: ", i);
        Eigen::Matrix4f transformLidar2Cam;
        transformLidar2Cam << vectorBuf[0], vectorBuf[1], vectorBuf[2], vectorBuf[3],
                              vectorBuf[4], vectorBuf[5], vectorBuf[6], vectorBuf[7],
                              vectorBuf[8], vectorBuf[9], vectorBuf[10], vectorBuf[11],
                              vectorBuf[12], vectorBuf[13], vectorBuf[14], vectorBuf[15];
        std::cout << transformLidar2Cam << std::endl;
        lidarCamParam.Tlidar2cam.emplace_back(transformLidar2Cam);
        if (distortionCoeffs.size() != 4 || intrinsics.size() != 4 || resolution.size() != 2 || outResolution.size() != 2) {
            ROS_ERROR("The number of distortionCoeffs, intrinsics, resolution and outResolution must be 4, 4, 2 and 2 respectively!");
            return -1;
        }
        double k1 = ((double)outResolution[0])/((double)resolution[0]);
        double k2 = ((double)outResolution[1])/((double)resolution[1]);
        cv::Mat distortMat = cv::Mat::zeros(1, 4, CV_64F);
        if (imageTopics[i].rfind("undistort") == std::string::npos) {
            distortMat = (cv::Mat_<double>(1, 4) << distortionCoeffs[0], distortionCoeffs[1], distortionCoeffs[2], distortionCoeffs[3]);
        }
        lidarCamParam.camParams.emplace_back(std::to_string(i + 1),
                                             cv::Size(outResolution[0], outResolution[1]),
                                            (cv::Mat_<double>(3, 3) << intrinsics[0]*k1, 0.0, intrinsics[2]*k1, 0.0, intrinsics[1]*k2, intrinsics[3]*k2, 0.0, 0.0, 1.0),
                                            distortMat);
    }

    if (useInterpolation) {
        ROS_INFO("Use interpolation!");
        LidarTwoImgsSyncer lidarTwoImgsSyncer(nh, lidarOdomTopic, pointcloudTopic, imageTopics, lidarCamParam, outputDir);
        if (!filteredClasses.empty())
            lidarTwoImgsSyncer.SetFilter(filteredClasses, filterRadius);
        lidarTwoImgsSyncer.Run();
    }
    else {
        ROS_INFO("Deny interpolation!");
        LidarOdomTwoImgsSyncer lidarOdomTwoImgsSyncer(nh, lidarOdomTopic, pointcloudTopic, imageTopics, lidarCamParam, outputDir);
        if (!filteredClasses.empty())
            lidarOdomTwoImgsSyncer.SetFilter(filteredClasses, filterRadius);
        ros::Rate rate(100);
        while (ros::ok())
        {
            rate.sleep();
            ros::spinOnce();
            if (lidarOdomTwoImgsSyncer.waitPeriod >= 0 && ++lidarOdomTwoImgsSyncer.waitPeriod > 1000) {
                ROS_WARN("No messages received for 1000 cycles, shutting down.");
                break;
            }
        }
    }

    return 0;
}
